#include "opration.h"
#include <math.h>
#include "arsx08.h"
/**
 ****************************************************************************
 * @Warning Without permission from the author,Not for commercial use
 * @File    readme.c
 * @Author  xiaowine@cee0.com
 * @date
 * @version V1.0
 *************************************************
 * @brief   标注系统信息
 ****************************************************************************
 * @attention
 * Powered By Xiaowine
 * <h2><center>&copy;  Copyright(C) cee0.com 2015-2019</center></h2>
 * All rights reserved
 *
 **/

ClkFg HIMReadClk  = {12, 0};
ClkFg HIMWriteClk = {11, 0};
ClkFg CANSendClk  = {5, 0};
ClkFg LEDSendClk  = {5, 0};

ClkFg HIMConnectFlashClk = {30, 0};
ClkFg CanConnectFlashClk = {30, 0};

_uFLAG     modbusFlag = {0};
_uFLAG     hmiKeyFlag = {0};
opration_t optData    = {0};

u16 CanCommunicationCount = 0xffff;
u16 HimCommunicationCount = 0xffff;
u16 beep10msCount         = 0;

u16     ttcTime, petTime;
int16_t range, speed;

u16    HIMSetStatus = 0;
double tan_angle    = 0;
double sqrt_angle   = 1;
/**
 *  y0 = |y-tan(angle)*x|/sqrt(1+tan(angle)*tan(angle))
 *  x0 = |x-tan(angle)*y|/sqrt(1+tan(angle)*tan(angle))
 **/

u8 colorSwitch = 0;

#define EEPROM_ADDRESS 32
#define EE_DATA_FLAG   0xa5a7

void readDataEeData(void)
{
    if (ee_CheckOk() == 1) {
        u8 eeData[50] = {0};
        EEpExistFlag  = 1;
        ee_ReadBytes(eeData, EEPROM_ADDRESS, 48);
        if (*((u16 *)&eeData[0]) == EE_DATA_FLAG) {
            memcpy(&optData, &eeData[2], sizeof(opration_t));
        } else {
            optData.startWarnDistance   = DISTANCE_WARN_START;
            optData.nearestWarnDistance = DISTANCE_WARN_LONGBEEP;
            optData.period              = 500;  // period x 10ms
            optData.rightWidth          = WidthRIGHT;
            optData.leftWidth           = -WidthLEFT;
            optData.angle               = 0;
            optData.warnStartTime       = TIME_WARN_START;
            optData.warnLongbeepTime    = TIME_WARN_LONGBEEP;
            optData.warnSpeed           = WARN_SPEED;
            optData.overSpeed           = OVER_SPEED;
        }
        tan_angle  = tan(optData.angle / 10);
        sqrt_angle = sqrt(1 + tan_angle * tan_angle);
    } else {
        EEpExistFlag = 0;
    }
}

void writeDataEeData(void)
{
    if (DataNeedRefreshFlag) {
        u8 eeData[50] = {0};
        memcpy(&eeData[2], &optData, sizeof(opration_t));
        DataNeedRefreshFlag  = 0;
        *((u16 *)&eeData[0]) = EE_DATA_FLAG;
        ee_WriteBytes(eeData, EEPROM_ADDRESS, 48);
    }
}

/**
 ************************************************************************************************************************
 * @Function 	:		void ReadHIM(void)
 * @author		: 	xiaowine@cee0.com
 * @brief 		: 	读取屏转速值
 * @param 		:
 * @Return  	:		None
 * @?�?�			:
 ************************************************************************************************************************
 */
void ReadHIM(void)
{
    if (HIMSetStatus) {
        if (HIMSetStatus & 0x0001) {
            if (MODH_ReadParam_03H(SlaveAddr, HimSParaAddress, 15) == 1) {
                memcpy(&optData, g_tVar[SlaveAddr - 1].P01, 30);
                HIMSetStatus &= ~0x0001;
                HimCommunicationCount = 0;

                tan_angle           = tan(optData.angle / 10);
                sqrt_angle          = sqrt(1 + tan_angle * tan_angle);
                DataNeedRefreshFlag = 1;
            } else {
            }
        }
    } else {
        if (MODH_ReadParam_03H(SlaveAddr, HimSetAddress, 2) == 1) {
            HIMSetStatus          = g_tVar[SlaveAddr - 1].P01[0];
            hmiKeyFlag.WORD_flag  = g_tVar[SlaveAddr - 1].P01[1];
            HimCommunicationCount = 0;
        } else {
        }
    }
}
/**
 ************************************************************************************************************************
 * @Function 	:		void WriteHIM(void)
 * @author		: 	xiaowine@cee0.com
 * @brief 		: 	写入屏油门值
 * @param 		:
 * @Return  	:		None
 * @?�?�			:
 ************************************************************************************************************************
 */
void WriteHIM(void)
{
    u8 HimDataBuff[50] = {0};
    if (!HMIOnLineFlag)
        return;

    if (!DataTransFlag) {
        HimDataBuff[0]  = optData.startWarnDistance >> 8;
        HimDataBuff[1]  = (u8)(optData.startWarnDistance & 0xff);
        HimDataBuff[2]  = optData.nearestWarnDistance >> 8;
        HimDataBuff[3]  = (u8)(optData.nearestWarnDistance & 0xff);
        HimDataBuff[4]  = optData.period >> 8;
        HimDataBuff[5]  = (u8)(optData.period & 0xff);
        HimDataBuff[6]  = optData.rightWidth >> 8;
        HimDataBuff[7]  = (u8)(optData.rightWidth & 0xff);
        HimDataBuff[8]  = optData.leftWidth >> 8;
        HimDataBuff[9]  = (u8)(optData.leftWidth & 0xff);
        HimDataBuff[10] = optData.angle >> 8;
        HimDataBuff[11] = (u8)(optData.angle & 0xff);
        HimDataBuff[12] = optData.warnStartTime >> 8;
        HimDataBuff[13] = (u8)(optData.warnStartTime & 0xff);
        HimDataBuff[14] = optData.warnLongbeepTime >> 8;
        HimDataBuff[15] = (u8)(optData.warnLongbeepTime & 0xff);
        HimDataBuff[16] = optData.data0 >> 8;
        HimDataBuff[17] = (u8)(optData.data0 & 0xff);
        HimDataBuff[18] = optData.data1 >> 8;
        HimDataBuff[19] = (u8)(optData.data1 & 0xff);
        HimDataBuff[20] = optData.data2 >> 8;
        HimDataBuff[21] = (u8)(optData.data2 & 0xff);
        HimDataBuff[22] = optData.D_VStatus >> 8;
        HimDataBuff[23] = (u8)(optData.D_VStatus & 0xff);
        HimDataBuff[24] = optData.pc9Period >> 8;
        HimDataBuff[25] = (u8)(optData.pc9Period & 0xff);

        if (MODH_WriteParam_10H(SlaveAddr, HimSParaAddress, 13, HimDataBuff)) {
            DataTransFlag = 1;
        }
        return;
    }

    if (HIMSetStatus & 0x1000) {
        HimDataBuff[0] = 0;
        HimDataBuff[1] = 0;
        if (MODH_WriteParam_10H(SlaveAddr, HimSetAddress, 1, HimDataBuff)) {
            HimCommunicationCount = 0;
            HIMSetStatus &= ~0x1000;
        }
        return;
    }

    if (radarChangeFlag == 0) {
        HimDataBuff[0] = range >> 8;
        HimDataBuff[1] = (u8)(range & 0xff);
        HimDataBuff[2] = speed >> 8;
        HimDataBuff[3] = (u8)(speed & 0xff);
        if (MODH_WriteParam_10H(SlaveAddr, HimRangAddress, 2, HimDataBuff)) {
            HimCommunicationCount = 0;
        }
    } else {
        // printf("radarChangeFlag:%d\r\n", radarChangeFlag);
        radarChangeFlag = 0;
        if (MODH_WriteParam_10H(SlaveAddr, HimDisplayAddress, 25, (u8 *)&objOncomingBuff[0])) {
            HimCommunicationCount = 0;
        }
    }
}
/**
 ************************************************************************************************************************
 * @Function 	:		void SendCan(void)
 * @author		: 	xiaowine@cee0.com
 * @brief 		: 	屏转速发送给控制器
 * @param 		:
 * @Return  	:	None
 * @?�?�	:
 ************************************************************************************************************************
 */
void SendCan(void)
{
    static u8 flag          = 0;
    u8        arsx08init[8] = {0xC8, 0x00, 0x00, 0x00, 0x08, 0x90, 0x00, 0x00};

    if (flag == 0) {
        Can_Send_Msg(0x0200, 0x08, arsx08init, 8, CAN_ID_STD, CAN_RTR_DATA);  // 发送8个字节
        flag = 1;
    }

    // if (flag)
    // {
    //     Can_Send_Msg(0x3f5, 0x06, msg3f5, 8, CAN_ID_STD, CAN_RTR_DATA); //发送8个字节
    // }
    // else
    // {
    //     Can_Send_Msg(0x3f6, 0x06, msg3f6, 8, CAN_ID_STD, CAN_RTR_DATA); //发送8个字节
    // }
}

static unsigned int CRC16(unsigned char *ptr, unsigned int count)
{
    unsigned int  crc = 0;
    unsigned char i;
    while (count > 0) {
        count--;
        crc = (crc ^ (((unsigned int)*ptr) << 8));
        for (i = 0; i < 8; i++) {
            if (crc & 0x8000)
                crc = ((crc << 1) ^ 0x1021);
            else
                crc <<= 1;
        }
        ptr++;
    }
    return crc;
}

void SendLED(void)
{
    char buff[50] = {0xFC, 0x0f, 0x01, 0x01, 0x07, 0x00, 0x00, 0x01, 0x03, 0xf0, 0xf0, 0x03, 0x08, 0x03, 0xff, 0x03, 0x72};
    if (colorSwitch == 0) {
        buff[9] = buff[10] = 0x0f;  // 绿色

        //			buff[9] = buff[10]=0xff;//黄色
        //			buff[9] = buff[10]=0xf0;//红色
    } else if (colorSwitch == 1) {
        buff[9] = buff[10] = 0x0f;  // 绿色

        //			buff[9] = buff[10]=0xff;//黄色
        //			buff[9] = buff[10]=0xf0;//红色
    } else if (colorSwitch == 2) {
        //			buff[9] = buff[10]=0x0f;//绿色
        //			buff[9] = buff[10]=0xff;//黄色

        buff[9] = buff[10] = 0xf0;  // 红色
    }
    // char buff[20] = {0xfc, 0x0f, 0x01, 0x02, 0x07, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x11, 0x00, 0x00};
    if (optData.D_VStatus) {
        u16 Obj_LongDispl = (objOncomingBuff[0].Obj_LongDispl0 << 8) | objOncomingBuff[0].Obj_LongDispl1;
        buff[11]          = Obj_LongDispl / 1000;
        Obj_LongDispl %= 1000;
        buff[12] = Obj_LongDispl / 100;
        Obj_LongDispl %= 100;
        buff[13]      = Obj_LongDispl / 10;
        Obj_LongDispl = CRC16((u8 *)buff, 15);
        buff[15]      = (Obj_LongDispl >> 8);
        buff[16]      = (Obj_LongDispl & 0xff);
    } else {
        u16 Obj_VerlLat = ((objOncomingBuff[0].Obj_VerlLat0 << 8) | objOncomingBuff[0].Obj_VerlLat1) / 10;

        buff[9] = Obj_VerlLat / 1000;
        Obj_VerlLat %= 1000;
        buff[10] = Obj_VerlLat / 100;
        Obj_VerlLat %= 100;
        buff[11] = Obj_VerlLat / 10;
        Obj_VerlLat %= 10;
        buff[13]    = Obj_VerlLat;
        buff[14]    = 0;
        Obj_VerlLat = CRC16_Modbus((u8 *)buff, 15);
        buff[15]    = (Obj_VerlLat >> 8);
        buff[16]    = (Obj_VerlLat & 0xff);

        // sprintf(buff, "V+%03d.%d", Obj_VerlLat / 10, Obj_VerlLat % 10);
        // sprintf(buff, "V+004.9");
        // printf("Obj_VerlLat:%d buff:%s\r\n",Obj_VerlLat,buff);
    }
    comSendBuf(COM5, (u8 *)buff, 17);
    // printf("%s\r\n", buff);
}

void BEEPopt(void)
{
    static u16 beepOffTime = 100;

    if (optData.D_VStatus == 0) {
        int16_t x0 = ((objOncomingBuff[0].Obj_LongDispl0 << 8) | objOncomingBuff[0].Obj_LongDispl1);
        int16_t v0 = (float)((objOncomingBuff[0].Obj_VerlLat0 << 8) | objOncomingBuff[0].Obj_VerlLat1);
        if (v0 < 0)
            v0 = 0;
        v0 = v0 * 3.6 / 100;
        if (x0 < 0) {
            x0 = 0 - x0;
        }
        if (v0 < 0)
            v0 = 0;
        if ((x0 < optData.startWarnDistance) && (x0 > 0)) {
            if (v0 < optData.warnSpeed) {
                PCout(8) = outPutOff;
                PGout(8) = outPutOff;
            } else if (v0 < optData.overSpeed) {
                PCout(8) = outPutOn;
                PGout(8) = outPutOff;
            } else {
                PCout(8) = outPutOn;
                PGout(8) = outPutOn;
            }
        } else {
            PCout(8) = outPutOff;
            PGout(8) = outPutOff;
        }

        if ((x0 > optData.startWarnDistance) || (x0 == 0) || (v0 < optData.warnSpeed)) {
            beepPin(outPutOn);
            BEEPFlag    = 1;
            colorSwitch = 0;
            warnFlag    = 0;
        } else if (x0 >= optData.nearestWarnDistance) {
            int16_t maxLong  = optData.startWarnDistance - optData.nearestWarnDistance;
            int16_t realLong = x0 - optData.nearestWarnDistance;
            u16     T_Long   = 500 / optData.period;
            warnFlag         = 1;
            if (maxLong < 1)
                maxLong = 1;
            if (realLong < 1)
                realLong = 1;
            if (T_Long < 1)
                T_Long = 1;
            beepOffTime = (1 + ((T_Long - 1) * realLong) / maxLong);
            if (BEEPFlag) {
                if (beep10msCount < BEEP_ON_TIME) {
                    return;
                }
                beepPin(outPutOn);
                BEEPFlag      = 0;
                beep10msCount = 0;
            } else {
                if (beep10msCount < beepOffTime) {
                    return;
                }
                beepPin(outPutOff);
                BEEPFlag      = 1;
                beep10msCount = 0;
            }

            colorSwitch = 1;
        } else {
            warnFlag    = 1;
            colorSwitch = 2;
            beepPin(outPutOff);
        }
    } else {
        float T0 = (float)((objOncomingBuff[0].Obj_VerlLat0 << 8) | objOncomingBuff[0].Obj_VerlLat1) / 100;

        if ((T0 > optData.warnStartTime) || (T0 <= 0)) {
            beepPin(outPutOn);
            BEEPFlag = 1;
            warnFlag = 0;
        } else if (T0 >= optData.warnLongbeepTime) {
            int16_t maxTime  = optData.warnStartTime - optData.warnLongbeepTime;
            int16_t realTime = T0 - optData.warnLongbeepTime;
            u16     T_Long   = 500 / optData.period;
            warnFlag         = 1;
            if (maxTime < 1)
                maxTime = 1;
            if (realTime < 1)
                realTime = 1;
            if (T_Long < 1)
                T_Long = 1;
            beepOffTime = (1 + ((T_Long - 1) * realTime) / maxTime) * 5;

            if (BEEPFlag) {
                if (beep10msCount < BEEP_ON_TIME) {
                    return;
                }
                beepPin(outPutOff);
                BEEPFlag = 0;
            } else {
                if (beep10msCount < beepOffTime) {
                    return;
                }
                beepPin(outPutOn);
                BEEPFlag = 1;
            }
            beep10msCount = 0;
        } else {
            warnFlag = 1;
            beepPin(outPutOff);
            BEEPFlag = 0;
        }
    }
}

/**
 ************************************************************************************************************************
 * @Function 	:		void TimeOutOpration(void)
 * @author		: 	xiaowine@cee0.com
 * @brief 		:
 * @param 		:
 * @Return  	:		None
 * @?�?�			:
 ************************************************************************************************************************
 */
void TimeOutOpration(void)
{
    /**
     * @author 	xiaowine@cee0.com
     * @date
     * @brief 	总线通讯超时
     *********************/
    if (HimCommunicationCount < 200) {
        if (HIMConnectFlashClk.uFLAG == 1) {
            HIMConnectFlashClk.uFLAG = 0;
            PAout(5) ^= 1;
        }
        HMIOnLineFlag = 1;
    } else {
        HMIOnLineFlag = 0;
        PAout(5)      = outPutOff;
    }
    if (CanCommunicationCount < 200) {
        if (CanConnectFlashClk.uFLAG == 1) {
            CanConnectFlashClk.uFLAG = 0;
            PAout(6) ^= 1;
        }
    } else {
        PAout(6) = outPutOff;
    }
}
/**
 ************************************************************************************************************************
 * @Function 	:		void OprateCanRec(CanRxMsg *recCache)
 * @author		: 	xiaowine@cee0.com
 * @brief 		: 	处理CAN接收数据
 * @param 		:
 * @Return  	:		None
 * @?�?�			:
 ************************************************************************************************************************
 */
void OprateCanRec(CanRxMsg *RecCache)
{
    if (RecCache->IDE == CAN_Id_Extended) {
        CanCommunicationCount = 0;
    } else if (RecCache->IDE == CAN_Id_Standard) {
        CanCommunicationCount = 0;
        if (RecCache->StdId == 0x6b0) {
            range = (((RecCache->Data[1] << 8) | RecCache->Data[2]) >> 3) * 2;
            if (((RecCache->Data[2] << 8) | RecCache->Data[3]) & 0x0400)
                speed = ((int16_t)((((RecCache->Data[2] << 8) | RecCache->Data[3]) & 0x07FF) | 0xF800)) * 2;
            else
                speed = ((int16_t)((((RecCache->Data[2] << 8) | RecCache->Data[3]) & 0x07FF) & 0x3FF)) * 2;
            printf("range:%d speed:%d\r\n", range, speed);
        }
        if (RecCache->StdId == 0x6f0) {
            ttcTime = (RecCache->Data[0] << 8) | RecCache->Data[1];
            petTime = (RecCache->Data[4] << 8) | RecCache->Data[5];
        }
        if (RecCache->StdId == 0x60a) {
            ARSObjStatusHandle(RecCache);
        }
        if (RecCache->StdId == 0x60b) {
            ARS_Obj_Handle(RecCache);
        }
        if (RecCache->StdId == 0x701) {
            ASR_Cluster_Handle(RecCache);
        }
    }
}

void beepPin(uint8_t state)
{
    if (state == outPutOn) {
        state = outPutOff;
    } else {
        state = outPutOn;
    }

    if (optData.data0 == optData.data1) {
        return;
    }
    // 選擇報警口
    if (optData.data1 == 1)
        PEout(0) = state;
    if (optData.data1 == 2)
        PBout(9) = state;
    if (optData.data1 == 3)
        PBout(8) = state;
    if (optData.data1 == 4)
        PBout(5) = state;
    if (optData.data1 == 5)
        PGout(15) = state;
    if (optData.data1 == 6)
        PGout(14) = state;
    if (optData.data1 == 7)
        PGout(13) = state;
    if (optData.data1 == 8)
        PGout(12) = state;
    if (optData.data1 == 9)
        PAout(11) = state;
    // if (optData.data1 == 10)
    //     PAout(10) = state;
    // if (optData.data1 == 11)
    //     PAout(9) = state;
    if (optData.data1 == 12)
        PAout(8) = state;
}
